/* 
 * FreeRTOS V6.0.2 - Copyright (C) 2010 Real Time Engineers Ltd.
 *
 * ***************************************************************************
 * * * * If you are: * * * * + New to FreeRTOS, * * + Wanting to learn FreeRTOS
 * or multitasking in general quickly * * + Looking for basic training, * * +
 * Wanting to improve your FreeRTOS skills and productivity * * * * then take a
 * look at the FreeRTOS eBook * * * * "Using the FreeRTOS Real Time Kernel - a
 * Practical Guide" * * http://www.FreeRTOS.org/Documentation * * * * A pdf
 * reference manual is also available.  Both are usually delivered * * to your
 * inbox within 20 minutes to two hours when purchased between 8am * * and 8pm
 * GMT (although please allow up to 24 hours in case of * * exceptional
 * circumstances).  Thank you for your support! * * *
 * ***************************************************************************
 *
 * This file is part of the FreeRTOS distribution.
 *
 * FreeRTOS is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License (version 2) as published by the
 * Free Software Foundation AND MODIFIED BY the FreeRTOS exception. ***NOTE***
 * The exception to the GPL is included to allow you to distribute a combined
 * work that includes FreeRTOS without being obliged to provide the source code
 * for proprietary components outside of the FreeRTOS kernel. FreeRTOS is
 * distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
 * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 * PARTICULAR PURPOSE.  See the GNU General Public License for more details.
 * You should have received a copy of the GNU General Public License and the
 * FreeRTOS license exception along with FreeRTOS; if not it can be viewed
 * here: http://www.freertos.org/a00114.html and also obtained by writing to
 * Richard Barry, contact details for whom are available on the FreeRTOS WEB
 * site.
 *
 * 1 tab == 4 spaces!
 *
 * http://www.FreeRTOS.org - Documentation, latest information, license and
 * contact details.
 *
 * http://www.SafeRTOS.com - A version that is certified for use in safety
 * critical systems.
 *
 * http://www.OpenRTOS.com - Commercial support, development, porting,
 * licensing and training services. */

#include <stdlib.h>
#include <string.h>

/* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining
 * all the API functions to use the MPU wrappers.  That should only be done when
 * task.h is included from an application file. */
#define MPU_WRAPPERS_INCLUDED_FROM_API_FILE

#include "FreeRTOS.h"
#include "task.h"
#include "croutine.h"

#include "queue.h"

#undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE

/*-----------------------------------------------------------
 * PUBLIC LIST API documented in list.h
 *----------------------------------------------------------*/

/* Constants used with the cRxLock and cTxLock structure members. */
#define queueUNLOCKED					( ( signed portBASE_TYPE ) -1 )
#define queueLOCKED_UNMODIFIED			( ( signed portBASE_TYPE ) 0 )

#define queueERRONEOUS_UNBLOCK			( -1 )

/* For internal use only. */
#define	queueSEND_TO_BACK				( 0 )
#define	queueSEND_TO_FRONT				( 1 )

/* Effectively make a union out of the xQUEUE structure. */
#define pxMutexHolder					pcTail
#define uxQueueType						pcHead
#define uxRecursiveCallCount			pcReadFrom
#define queueQUEUE_IS_MUTEX				NULL

/* Semaphores do not actually store or copy data, so have an items size of
 * zero. */
#define queueSEMAPHORE_QUEUE_ITEM_LENGTH ( 0 )
#define queueDONT_BLOCK					 ( ( portTickType ) 0 )
#define queueMUTEX_GIVE_BLOCK_TIME		 ( ( portTickType ) 0 )

/* 
 * Definition of the queue used by the scheduler.
 * Items are queued by copy, not reference.
 */
typedef struct QueueDefinition {
    signed char *pcHead;        /* < Points to the beginning of the queue
                                 * storage area. */
    signed char *pcTail;        /* < Points to the byte at the end of the queue
                                 * storage area.  Once more byte is allocated
                                 * than necessary to store the queue items, this
                                 * * * * * * * * * * * * * * * * * * * * * * * is 
                                 * used as a marker. */

    signed char *pcWriteTo;     /* < Points to the free next place in the
                                 * storage area. */
    signed char *pcReadFrom;    /* < Points to the last place that a queued
                                 * item was read from. */

    xList xTasksWaitingToSend;  /* < List of tasks that are blocked waiting to
                                 * post onto this queue.  Stored in priority
                                 * order. */
    xList xTasksWaitingToReceive;   /* < List of tasks that are blocked waiting
                                     * to read from this queue.  Stored in
                                     * priority order. */

    volatile unsigned portBASE_TYPE uxMessagesWaiting;  /* < The number of
                                                         * items currently in
                                                         * the queue. */
    unsigned portBASE_TYPE uxLength;    /* < The length of the queue defined as
                                         * the number of items it will hold, not
                                         * * * * * * * * * * * * * * * * * * * *
                                         * * * * the number of bytes. */
    unsigned portBASE_TYPE uxItemSize;  /* < The size of each items that the
                                         * queue will hold. */

    signed portBASE_TYPE xRxLock;   /* < Stores the number of items received
                                     * from the queue (removed from the queue)
                                     * while the queue was locked.  Set to
                                     * queueUNLOCKED when the queue is not
                                     * locked. */
    signed portBASE_TYPE xTxLock;   /* < Stores the number of items transmitted
                                     * to the queue (added to the queue) while
                                     * the queue was locked.  Set to
                                     * queueUNLOCKED when the queue is not
                                     * locked. */

} xQUEUE;

/*-----------------------------------------------------------*/

/* 
 * Inside this file xQueueHandle is a pointer to a xQUEUE structure.
 * To keep the definition private the API header file defines it as a
 * pointer to void.
 */
typedef xQUEUE *xQueueHandlePrv;

/* 
 * Prototypes for public functions are included here so we don't have to
 * include the API header file (as it defines xQueueHandle differently).  These
 * functions are documented in the API header file.
 */
xQueueHandle xQueueCreate(unsigned portBASE_TYPE uxQueueLength,
                          unsigned portBASE_TYPE uxItemSize)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueGenericSend(xQueueHandle xQueue,
                                            const void *const pvItemToQueue,
                                            portTickType xTicksToWait,
                                            portBASE_TYPE xCopyPosition)
    PRIVILEGED_FUNCTION;
     unsigned portBASE_TYPE uxQueueMessagesWaiting(const xQueueHandle pxQueue)
    PRIVILEGED_FUNCTION;
     void vQueueDelete(xQueueHandle xQueue) PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueGenericSendFromISR(xQueueHandle pxQueue,
                                                   const void *const
                                                   pvItemToQueue,
                                                   signed portBASE_TYPE *
                                                   pxHigherPriorityTaskWoken,
                                                   portBASE_TYPE xCopyPosition)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueGenericReceive(xQueueHandle pxQueue,
                                               void *const pvBuffer,
                                               portTickType xTicksToWait,
                                               portBASE_TYPE xJustPeeking)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueReceiveFromISR(xQueueHandle pxQueue,
                                               void *const pvBuffer,
                                               signed portBASE_TYPE *
                                               pxTaskWoken) PRIVILEGED_FUNCTION;
     xQueueHandle xQueueCreateMutex(void) PRIVILEGED_FUNCTION;
     xQueueHandle xQueueCreateCountingSemaphore(unsigned portBASE_TYPE
                                                uxCountValue,
                                                unsigned portBASE_TYPE
                                                uxInitialCount)
    PRIVILEGED_FUNCTION;
     portBASE_TYPE xQueueTakeMutexRecursive(xQueueHandle xMutex,
                                            portTickType xBlockTime)
    PRIVILEGED_FUNCTION;
     portBASE_TYPE xQueueGiveMutexRecursive(xQueueHandle xMutex)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueAltGenericSend(xQueueHandle pxQueue,
                                               const void *const pvItemToQueue,
                                               portTickType xTicksToWait,
                                               portBASE_TYPE xCopyPosition)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueAltGenericReceive(xQueueHandle pxQueue,
                                                  void *const pvBuffer,
                                                  portTickType xTicksToWait,
                                                  portBASE_TYPE xJustPeeking)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueIsQueueEmptyFromISR(const xQueueHandle pxQueue)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueIsQueueFullFromISR(const xQueueHandle pxQueue)
    PRIVILEGED_FUNCTION;
     unsigned portBASE_TYPE uxQueueMessagesWaitingFromISR(const xQueueHandle
                                                          pxQueue)
    PRIVILEGED_FUNCTION;

/* 
 * Co-routine queue functions differ from task queue functions.  Co-routines are
 * an optional component.
 */
#if configUSE_CO_ROUTINES == 1
     signed portBASE_TYPE xQueueCRSendFromISR(xQueueHandle pxQueue,
                                              const void *pvItemToQueue,
                                              signed portBASE_TYPE
                                              xCoRoutinePreviouslyWoken)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueCRReceiveFromISR(xQueueHandle pxQueue,
                                                 void *pvBuffer,
                                                 signed portBASE_TYPE *
                                                 pxTaskWoken)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueCRSend(xQueueHandle pxQueue,
                                       const void *pvItemToQueue,
                                       portTickType xTicksToWait)
    PRIVILEGED_FUNCTION;
     signed portBASE_TYPE xQueueCRReceive(xQueueHandle pxQueue, void *pvBuffer,
                                          portTickType xTicksToWait)
    PRIVILEGED_FUNCTION;
#endif

/* 
 * The queue registry is just a means for kernel aware debuggers to locate
 * queue structures.  It has no other purpose so is an optional component.
 */
#if configQUEUE_REGISTRY_SIZE > 0

    /* The type stored within the queue registry array.  This allows a name to
     * be assigned to each queue making kernel aware debugging a little more
     * user friendly. */
     typedef struct QUEUE_REGISTRY_ITEM {
         signed char *pcQueueName;
         xQueueHandlePrv xHandle;
     } xQueueRegistryItem;

    /* The queue registry is simply an array of xQueueRegistryItem structures.
     * The pcQueueName member of a structure being NULL is indicative of the
     * array position being vacant. */
     xQueueRegistryItem xQueueRegistry[configQUEUE_REGISTRY_SIZE];

    /* Removes a queue from the registry by simply setting the pcQueueName
     * member to NULL. */
     static void vQueueUnregisterQueue(xQueueHandle xQueue) PRIVILEGED_FUNCTION;
     void vQueueAddToRegistry(xQueueHandle xQueue,
                              signed char *pcQueueName) PRIVILEGED_FUNCTION;
#endif

/* 
 * Unlocks a queue locked by a call to prvLockQueue.  Locking a queue does not
 * prevent an ISR from adding or removing items to the queue, but does prevent
 * an ISR from removing tasks from the queue event lists.  If an ISR finds a
 * queue is locked it will instead increment the appropriate queue lock count
 * to indicate that a task may require unblocking.  When the queue in unlocked
 * these lock counts are inspected, and the appropriate action taken.
 */
     static void prvUnlockQueue(xQueueHandle pxQueue) PRIVILEGED_FUNCTION;

/* 
 * Uses a critical section to determine if there is any data in a queue.
 *
 * @return pdTRUE if the queue contains no items, otherwise pdFALSE.
 */
     static signed portBASE_TYPE prvIsQueueEmpty(const xQueueHandle pxQueue)
    PRIVILEGED_FUNCTION;

/* 
 * Uses a critical section to determine if there is any space in a queue.
 *
 * @return pdTRUE if there is no space, otherwise pdFALSE;
 */
     static signed portBASE_TYPE prvIsQueueFull(const xQueueHandle pxQueue)
    PRIVILEGED_FUNCTION;

/* 
 * Copies an item into the queue, either at the front of the queue or the
 * back of the queue.
 */
     static void prvCopyDataToQueue(xQUEUE * pxQueue, const void *pvItemToQueue,
                                    portBASE_TYPE xPosition)
    PRIVILEGED_FUNCTION;

/* 
 * Copies an item out of a queue.
 */
     static void prvCopyDataFromQueue(xQUEUE * const pxQueue,
                                      const void *pvBuffer) PRIVILEGED_FUNCTION;

/*-----------------------------------------------------------*/

/* 
 * Macro to mark a queue as locked.  Locking a queue prevents an ISR from
 * accessing the queue event lists.
 */
#define prvLockQueue( pxQueue )							\
{														\
	taskENTER_CRITICAL();								\
	{													\
		if( pxQueue->xRxLock == queueUNLOCKED )			\
		{												\
			pxQueue->xRxLock = queueLOCKED_UNMODIFIED;	\
		}												\
		if( pxQueue->xTxLock == queueUNLOCKED )			\
		{												\
			pxQueue->xTxLock = queueLOCKED_UNMODIFIED;	\
		}												\
	}													\
	taskEXIT_CRITICAL();								\
}

/*-----------------------------------------------------------*/


/*-----------------------------------------------------------
 * PUBLIC QUEUE MANAGEMENT API documented in queue.h
 *----------------------------------------------------------*/

     xQueueHandle xQueueCreate(unsigned portBASE_TYPE uxQueueLength,
                               unsigned portBASE_TYPE uxItemSize)
{
    xQUEUE *pxNewQueue;
    size_t xQueueSizeInBytes;

    /* Allocate the new queue structure. */
    if (uxQueueLength > (unsigned portBASE_TYPE)0) {
        pxNewQueue = (xQUEUE *) pvPortMalloc(sizeof(xQUEUE));
        if (pxNewQueue != NULL) {
            /* Create the list of pointers to queue items.  The queue is one
             * byte longer than asked for to make wrap checking easier/faster. */
            xQueueSizeInBytes =
                (size_t) (uxQueueLength * uxItemSize) + (size_t) 1;

            pxNewQueue->pcHead = (signed char *)pvPortMalloc(xQueueSizeInBytes);
            if (pxNewQueue->pcHead != NULL) {
                /* Initialise the queue members as described above where the
                 * queue type is defined. */
                pxNewQueue->pcTail =
                    pxNewQueue->pcHead + (uxQueueLength * uxItemSize);
                pxNewQueue->uxMessagesWaiting = 0;
                pxNewQueue->pcWriteTo = pxNewQueue->pcHead;
                pxNewQueue->pcReadFrom =
                    pxNewQueue->pcHead + ((uxQueueLength - 1) * uxItemSize);
                pxNewQueue->uxLength = uxQueueLength;
                pxNewQueue->uxItemSize = uxItemSize;
                pxNewQueue->xRxLock = queueUNLOCKED;
                pxNewQueue->xTxLock = queueUNLOCKED;

                /* Likewise ensure the event queues start with the correct
                 * state. */
                vListInitialise(&(pxNewQueue->xTasksWaitingToSend));
                vListInitialise(&(pxNewQueue->xTasksWaitingToReceive));

                traceQUEUE_CREATE(pxNewQueue);
                return pxNewQueue;
            } else {
                traceQUEUE_CREATE_FAILED();
                vPortFree(pxNewQueue);
            }
        }
    }

    /* Will only reach here if we could not allocate enough memory or no memory
     * was required. */
    return NULL;
}

/*-----------------------------------------------------------*/

#if ( configUSE_MUTEXES == 1 )

xQueueHandle xQueueCreateMutex(void)
{
    xQUEUE *pxNewQueue;

    /* Allocate the new queue structure. */
    pxNewQueue = (xQUEUE *) pvPortMalloc(sizeof(xQUEUE));
    if (pxNewQueue != NULL) {
        /* Information required for priority inheritance. */
        pxNewQueue->pxMutexHolder = NULL;
        pxNewQueue->uxQueueType = queueQUEUE_IS_MUTEX;

        /* Queues used as a mutex no data is actually copied into or out of the
         * queue. */
        pxNewQueue->pcWriteTo = NULL;
        pxNewQueue->pcReadFrom = NULL;

        /* Each mutex has a length of 1 (like a binary semaphore) and an item
         * size of 0 as nothing is actually copied into or out of the mutex. */
        pxNewQueue->uxMessagesWaiting = 0;
        pxNewQueue->uxLength = 1;
        pxNewQueue->uxItemSize = 0;
        pxNewQueue->xRxLock = queueUNLOCKED;
        pxNewQueue->xTxLock = queueUNLOCKED;

        /* Ensure the event queues start with the correct state. */
        vListInitialise(&(pxNewQueue->xTasksWaitingToSend));
        vListInitialise(&(pxNewQueue->xTasksWaitingToReceive));

        /* Start with the semaphore in the expected state. */
        xQueueGenericSend(pxNewQueue, NULL, 0, queueSEND_TO_BACK);

        traceCREATE_MUTEX(pxNewQueue);
    } else {
        traceCREATE_MUTEX_FAILED();
    }

    return (xQueueHandle) pxNewQueue;
}

#endif /* configUSE_MUTEXES */

/*-----------------------------------------------------------*/

#if configUSE_RECURSIVE_MUTEXES == 1

portBASE_TYPE xQueueGiveMutexRecursive(xQueueHandle pxMutexParam)
{
    portBASE_TYPE xReturn;

    xQueueHandlePrv pxMutex;
    pxMutex = (xQueueHandlePrv) pxMutexParam;

    /* If this is the task that holds the mutex then pxMutexHolder will not
     * change outside of this task.  If this task does not hold the mutex then
     * pxMutexHolder can never coincidentally equal the tasks handle, and as
     * this is the only condition we are interested in it does not matter if
     * pxMutexHolder is accessed simultaneously by another task.  Therefore no
     * mutual exclusion is required to test the pxMutexHolder variable. */
    if (pxMutex->pxMutexHolder == xTaskGetCurrentTaskHandle()) {
        traceGIVE_MUTEX_RECURSIVE(pxMutex);

        /* uxRecursiveCallCount cannot be zero if pxMutexHolder is equal to the
         * task handle, therefore no underflow check is required.  Also,
         * uxRecursiveCallCount is only modified by the mutex holder, and as
         * there can only be one, no mutual exclusion is required to modify the
         * uxRecursiveCallCount member. */
        (pxMutex->uxRecursiveCallCount)--;

        /* Have we unwound the call count? */
        if (pxMutex->uxRecursiveCallCount == 0) {
            /* Return the mutex.  This will automatically unblock any other
             * task that might be waiting to access the mutex. */
            xQueueGenericSend(pxMutex, NULL, queueMUTEX_GIVE_BLOCK_TIME,
                              queueSEND_TO_BACK);
        }

        xReturn = pdPASS;
    } else {
        /* We cannot give the mutex because we are not the holder. */
        xReturn = pdFAIL;

        traceGIVE_MUTEX_RECURSIVE_FAILED(pxMutex);
    }

    return xReturn;
}

#endif /* configUSE_RECURSIVE_MUTEXES */

/*-----------------------------------------------------------*/

#if configUSE_RECURSIVE_MUTEXES == 1

portBASE_TYPE xQueueTakeMutexRecursive(xQueueHandle pxMutexParam,
                                       portTickType xBlockTime)
{
    portBASE_TYPE xReturn;
    xQueueHandlePrv pxMutex = (xQueueHandlePrv) pxMutexParam;

    /* Comments regarding mutual exclusion as per those within
     * xQueueGiveMutexRecursive(). */

    traceTAKE_MUTEX_RECURSIVE(pxMutex);

    if (pxMutex->pxMutexHolder == xTaskGetCurrentTaskHandle()) {
        (pxMutex->uxRecursiveCallCount)++;
        xReturn = pdPASS;
    } else {
        xReturn = xQueueGenericReceive(pxMutex, NULL, xBlockTime, pdFALSE);

        /* pdPASS will only be returned if we successfully obtained the mutex,
         * we may have blocked to reach here. */
        if (xReturn == pdPASS) {
            (pxMutex->uxRecursiveCallCount)++;
        }
    }

    return xReturn;
}

#endif /* configUSE_RECURSIVE_MUTEXES */

/*-----------------------------------------------------------*/

#if configUSE_COUNTING_SEMAPHORES == 1

xQueueHandle xQueueCreateCountingSemaphore(unsigned portBASE_TYPE uxCountValue,
                                           unsigned portBASE_TYPE
                                           uxInitialCount)
{
    xQueueHandlePrv pxHandle;

    pxHandle =
        xQueueCreate((unsigned portBASE_TYPE)uxCountValue,
                     queueSEMAPHORE_QUEUE_ITEM_LENGTH);

    if (pxHandle != NULL) {
        pxHandle->uxMessagesWaiting = uxInitialCount;

        traceCREATE_COUNTING_SEMAPHORE();
    } else {
        traceCREATE_COUNTING_SEMAPHORE_FAILED();
    }

    return (xQueueHandle) pxHandle;
}

#endif /* configUSE_COUNTING_SEMAPHORES */

/*-----------------------------------------------------------*/

signed portBASE_TYPE xQueueGenericSend(xQueueHandle pxQueueParam,
                                       const void *const pvItemToQueue,
                                       portTickType xTicksToWait,
                                       portBASE_TYPE xCopyPosition)
{
    signed portBASE_TYPE xEntryTimeSet = pdFALSE;
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;
    xTimeOutType xTimeOut;

    /* This function relaxes the coding standard somewhat to allow return
     * statements within the function itself.  This is done in the interest of
     * execution time efficiency. */
    for (;;) {
        taskENTER_CRITICAL();
        {
            /* Is there room on the queue now? To be running we must be the
             * highest priority task wanting to access the queue. */
            if (pxQueue->uxMessagesWaiting < pxQueue->uxLength) {
                traceQUEUE_SEND(pxQueue);
                prvCopyDataToQueue(pxQueue, pvItemToQueue, xCopyPosition);

                /* If there was a task waiting for data to arrive on the queue
                 * then unblock it now. */
                if (listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToReceive)) ==
                    pdFALSE) {
                    if (xTaskRemoveFromEventList
                        (&(pxQueue->xTasksWaitingToReceive)) == pdTRUE) {
                        /* The unblocked task has a priority higher than our
                         * own so yield immediately.  Yes it is ok to do this
                         * from within the critical section - the kernel takes
                         * care of that. */
                        portYIELD_WITHIN_API();
                    }
                }

                taskEXIT_CRITICAL();

                /* Return to the original privilege level before exiting the
                 * function. */
                return pdPASS;
            } else {
                if (xTicksToWait == (portTickType) 0) {
                    /* The queue was full and no block time is specified (or
                     * the block time has expired) so leave now. */
                    taskEXIT_CRITICAL();

                    /* Return to the original privilege level before exiting
                     * the function. */
                    traceQUEUE_SEND_FAILED(pxQueue);
                    return errQUEUE_FULL;
                } else if (xEntryTimeSet == pdFALSE) {
                    /* The queue was full and a block time was specified so
                     * configure the timeout structure. */
                    vTaskSetTimeOutState(&xTimeOut);
                    xEntryTimeSet = pdTRUE;
                }
            }
        }
        taskEXIT_CRITICAL();

        /* Interrupts and other tasks can send to and receive from the queue
         * now the critical section has been exited. */

        vTaskSuspendAll();
        prvLockQueue(pxQueue);

        /* Update the timeout state to see if it has expired yet. */
        if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait) == pdFALSE) {
            if (prvIsQueueFull(pxQueue)) {
                traceBLOCKING_ON_QUEUE_SEND(pxQueue);
                vTaskPlaceOnEventList(&(pxQueue->xTasksWaitingToSend),
                                      xTicksToWait);

                /* Unlocking the queue means queue events can effect the event
                 * list.  It is possible that interrupts occurring now remove
                 * this task from the event list again - but as the scheduler
                 * is suspended the task will go onto the pending ready last
                 * instead of the actual ready list. */
                prvUnlockQueue(pxQueue);

                /* Resuming the scheduler will move tasks from the pending
                 * ready list into the ready list - so it is feasible that this
                 * task is already in a ready list before it yields - in which
                 * case the yield will not cause a context switch unless there
                 * is also a higher priority task in the pending ready list. */
                if (!xTaskResumeAll()) {
                    portYIELD_WITHIN_API();
                }
            } else {
                /* Try again. */
                prvUnlockQueue(pxQueue);
                (void)xTaskResumeAll();
            }
        } else {
            /* The timeout has expired. */
            prvUnlockQueue(pxQueue);
            (void)xTaskResumeAll();

            /* Return to the original privilege level before exiting the
             * function. */
            traceQUEUE_SEND_FAILED(pxQueue);
            return errQUEUE_FULL;
        }
    }
}

/*-----------------------------------------------------------*/

#if configUSE_ALTERNATIVE_API == 1

signed portBASE_TYPE xQueueAltGenericSend(xQueueHandle pxQueueParam,
                                          const void *const pvItemToQueue,
                                          portTickType xTicksToWait,
                                          portBASE_TYPE xCopyPosition)
{
    signed portBASE_TYPE xEntryTimeSet = pdFALSE;
    xQueueHandlePrv pxQueue;
    pxQueue = (xQueueHandlePrv) pxQueueParam;


    xTimeOutType xTimeOut;

    for (;;) {
        taskENTER_CRITICAL();
        {
            /* Is there room on the queue now? To be running we must be the
             * highest priority task wanting to access the queue. */
            if (pxQueue->uxMessagesWaiting < pxQueue->uxLength) {
                traceQUEUE_SEND(pxQueue);
                prvCopyDataToQueue(pxQueue, pvItemToQueue, xCopyPosition);

                /* If there was a task waiting for data to arrive on the queue
                 * then unblock it now. */
                if (listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToReceive)) ==
                    pdFALSE) {
                    if (xTaskRemoveFromEventList
                        (&(pxQueue->xTasksWaitingToReceive)) == pdTRUE) {
                        /* The unblocked task has a priority higher than our
                         * own so yield immediately. */
                        portYIELD_WITHIN_API();
                    }
                }

                taskEXIT_CRITICAL();
                return pdPASS;
            } else {
                if (xTicksToWait == (portTickType) 0) {
                    taskEXIT_CRITICAL();
                    return errQUEUE_FULL;
                } else if (xEntryTimeSet == pdFALSE) {
                    vTaskSetTimeOutState(&xTimeOut);
                    xEntryTimeSet = pdTRUE;
                }
            }
        }
        taskEXIT_CRITICAL();

        taskENTER_CRITICAL();
        {
            if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait) == pdFALSE) {
                if (prvIsQueueFull(pxQueue)) {
                    traceBLOCKING_ON_QUEUE_SEND(pxQueue);
                    vTaskPlaceOnEventList(&(pxQueue->xTasksWaitingToSend),
                                          xTicksToWait);
                    portYIELD_WITHIN_API();
                }
            } else {
                taskEXIT_CRITICAL();
                traceQUEUE_SEND_FAILED(pxQueue);
                return errQUEUE_FULL;
            }
        }
        taskEXIT_CRITICAL();
    }
}

#endif /* configUSE_ALTERNATIVE_API */

/*-----------------------------------------------------------*/

#if configUSE_ALTERNATIVE_API == 1

signed portBASE_TYPE xQueueAltGenericReceive(xQueueHandle pxQueueParam,
                                             void *const pvBuffer,
                                             portTickType xTicksToWait,
                                             portBASE_TYPE xJustPeeking)
{
    signed portBASE_TYPE xEntryTimeSet = pdFALSE;
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    xTimeOutType xTimeOut;
    signed char *pcOriginalReadPosition;

    for (;;) {
        taskENTER_CRITICAL();
        {
            if (pxQueue->uxMessagesWaiting > (unsigned portBASE_TYPE)0) {
                /* Remember our read position in case we are just peeking. */
                pcOriginalReadPosition = pxQueue->pcReadFrom;

                prvCopyDataFromQueue(pxQueue, pvBuffer);

                if (xJustPeeking == pdFALSE) {
                    traceQUEUE_RECEIVE(pxQueue);

                    /* We are actually removing data. */
                    --(pxQueue->uxMessagesWaiting);

#if ( configUSE_MUTEXES == 1 )
                    {
                        if (pxQueue->uxQueueType == queueQUEUE_IS_MUTEX) {
                            /* Record the information required to implement
                             * priority inheritance should it become necessary. */
                            pxQueue->pxMutexHolder =
                                xTaskGetCurrentTaskHandle();
                        }
                    }
#endif

                    if (listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToSend)) ==
                        pdFALSE) {
                        if (xTaskRemoveFromEventList
                            (&(pxQueue->xTasksWaitingToSend)) == pdTRUE) {
                            portYIELD_WITHIN_API();
                        }
                    }
                } else {
                    traceQUEUE_PEEK(pxQueue);

                    /* We are not removing the data, so reset our read pointer. */
                    pxQueue->pcReadFrom = pcOriginalReadPosition;

                    /* The data is being left in the queue, so see if there are
                     * any other tasks waiting for the data. */
                    if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToReceive))) {
                        /* Tasks that are removed from the event list will get
                         * added to the pending ready list as the scheduler is
                         * still suspended. */
                        if (xTaskRemoveFromEventList
                            (&(pxQueue->xTasksWaitingToReceive)) != pdFALSE) {
                            /* The task waiting has a higher priority than this
                             * task. */
                            portYIELD_WITHIN_API();
                        }
                    }

                }

                taskEXIT_CRITICAL();
                return pdPASS;
            } else {
                if (xTicksToWait == (portTickType) 0) {
                    taskEXIT_CRITICAL();
                    traceQUEUE_RECEIVE_FAILED(pxQueue);
                    return errQUEUE_EMPTY;
                } else if (xEntryTimeSet == pdFALSE) {
                    vTaskSetTimeOutState(&xTimeOut);
                    xEntryTimeSet = pdTRUE;
                }
            }
        }
        taskEXIT_CRITICAL();

        taskENTER_CRITICAL();
        {
            if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait) == pdFALSE) {
                if (prvIsQueueEmpty(pxQueue)) {
                    traceBLOCKING_ON_QUEUE_RECEIVE(pxQueue);

#if ( configUSE_MUTEXES == 1 )
                    {
                        if (pxQueue->uxQueueType == queueQUEUE_IS_MUTEX) {
                            portENTER_CRITICAL();
                            vTaskPriorityInherit((void *)
                                                 pxQueue->pxMutexHolder);
                            portEXIT_CRITICAL();
                        }
                    }
#endif

                    vTaskPlaceOnEventList(&(pxQueue->xTasksWaitingToReceive),
                                          xTicksToWait);
                    portYIELD_WITHIN_API();
                }
            } else {
                taskEXIT_CRITICAL();
                traceQUEUE_RECEIVE_FAILED(pxQueue);
                return errQUEUE_EMPTY;
            }
        }
        taskEXIT_CRITICAL();
    }
}


#endif /* configUSE_ALTERNATIVE_API */

/*-----------------------------------------------------------*/

signed portBASE_TYPE xQueueGenericSendFromISR(xQueueHandle pxQueueParam,
                                              const void *const pvItemToQueue,
                                              signed portBASE_TYPE *
                                              pxHigherPriorityTaskWoken,
                                              portBASE_TYPE xCopyPosition)
{
    signed portBASE_TYPE xReturn;
    unsigned portBASE_TYPE uxSavedInterruptStatus;
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;


    /* Similar to xQueueGenericSend, except we don't block if there is no room
     * in the queue.  Also we don't directly wake a task that was blocked on a
     * queue read, instead we return a flag to say whether a context switch is
     * required or not (i.e. has a task with a higher priority than us been
     * woken by this post). */
    uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR();
    {
        if (pxQueue->uxMessagesWaiting < pxQueue->uxLength) {
            traceQUEUE_SEND_FROM_ISR(pxQueue);

            prvCopyDataToQueue(pxQueue, pvItemToQueue, xCopyPosition);

            /* If the queue is locked we do not alter the event list.  This
             * will be done when the queue is unlocked later. */
            if (pxQueue->xTxLock == queueUNLOCKED) {
                if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToReceive))) {
                    if (xTaskRemoveFromEventList
                        (&(pxQueue->xTasksWaitingToReceive)) != pdFALSE) {
                        /* The task waiting has a higher priority so record
                         * that a context switch is required. */
                        *pxHigherPriorityTaskWoken = pdTRUE;
                    }
                }
            } else {
                /* Increment the lock count so the task that unlocks the queue
                 * knows that data was posted while it was locked. */
                ++(pxQueue->xTxLock);
            }

            xReturn = pdPASS;
        } else {
            traceQUEUE_SEND_FROM_ISR_FAILED(pxQueue);
            xReturn = errQUEUE_FULL;
        }
    }
    portCLEAR_INTERRUPT_MASK_FROM_ISR(uxSavedInterruptStatus);

    return xReturn;
}

/*-----------------------------------------------------------*/

signed portBASE_TYPE xQueueGenericReceive(xQueueHandle pxQueueParam,
                                          void *const pvBuffer,
                                          portTickType xTicksToWait,
                                          portBASE_TYPE xJustPeeking)
{
    signed portBASE_TYPE xEntryTimeSet = pdFALSE;
    xTimeOutType xTimeOut;
    signed char *pcOriginalReadPosition;
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    /* This function relaxes the coding standard somewhat to allow return
     * statements within the function itself.  This is done in the interest of
     * execution time efficiency. */

    for (;;) {
        taskENTER_CRITICAL();
        {
            /* Is there data in the queue now? To be running we must be the
             * highest priority task wanting to access the queue. */
            if (pxQueue->uxMessagesWaiting > (unsigned portBASE_TYPE)0) {
                /* Remember our read position in case we are just peeking. */
                pcOriginalReadPosition = pxQueue->pcReadFrom;

                prvCopyDataFromQueue(pxQueue, pvBuffer);

                if (xJustPeeking == pdFALSE) {
                    traceQUEUE_RECEIVE(pxQueue);

                    /* We are actually removing data. */
                    --(pxQueue->uxMessagesWaiting);

#if ( configUSE_MUTEXES == 1 )
                    {
                        if (pxQueue->uxQueueType == queueQUEUE_IS_MUTEX) {
                            /* Record the information required to implement
                             * priority inheritance should it become necessary. */
                            pxQueue->pxMutexHolder =
                                xTaskGetCurrentTaskHandle();
                        }
                    }
#endif

                    if (listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToSend)) ==
                        pdFALSE) {
                        if (xTaskRemoveFromEventList
                            (&(pxQueue->xTasksWaitingToSend)) == pdTRUE) {
                            portYIELD_WITHIN_API();
                        }
                    }
                } else {
                    traceQUEUE_PEEK(pxQueue);

                    /* We are not removing the data, so reset our read pointer. */
                    pxQueue->pcReadFrom = pcOriginalReadPosition;

                    /* The data is being left in the queue, so see if there are
                     * any other tasks waiting for the data. */
                    if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToReceive))) {
                        /* Tasks that are removed from the event list will get
                         * added to the pending ready list as the scheduler is
                         * still suspended. */
                        if (xTaskRemoveFromEventList
                            (&(pxQueue->xTasksWaitingToReceive)) != pdFALSE) {
                            /* The task waiting has a higher priority than this
                             * task. */
                            portYIELD_WITHIN_API();
                        }
                    }

                }

                taskEXIT_CRITICAL();
                return pdPASS;
            } else {
                if (xTicksToWait == (portTickType) 0) {
                    /* The queue was empty and no block time is specified (or
                     * the block time has expired) so leave now. */
                    taskEXIT_CRITICAL();
                    traceQUEUE_RECEIVE_FAILED(pxQueue);
                    return errQUEUE_EMPTY;
                } else if (xEntryTimeSet == pdFALSE) {
                    /* The queue was empty and a block time was specified so
                     * configure the timeout structure. */
                    vTaskSetTimeOutState(&xTimeOut);
                    xEntryTimeSet = pdTRUE;
                }
            }
        }
        taskEXIT_CRITICAL();

        /* Interrupts and other tasks can send to and receive from the queue
         * now the critical section has been exited. */

        vTaskSuspendAll();
        prvLockQueue(pxQueue);

        /* Update the timeout state to see if it has expired yet. */
        if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait) == pdFALSE) {
            if (prvIsQueueEmpty(pxQueue)) {
                traceBLOCKING_ON_QUEUE_RECEIVE(pxQueue);

#if ( configUSE_MUTEXES == 1 )
                {
                    if (pxQueue->uxQueueType == queueQUEUE_IS_MUTEX) {
                        portENTER_CRITICAL();
                        {
                            vTaskPriorityInherit((void *)
                                                 pxQueue->pxMutexHolder);
                        }
                        portEXIT_CRITICAL();
                    }
                }
#endif

                vTaskPlaceOnEventList(&(pxQueue->xTasksWaitingToReceive),
                                      xTicksToWait);
                prvUnlockQueue(pxQueue);
                if (!xTaskResumeAll()) {
                    portYIELD_WITHIN_API();
                }
            } else {
                /* Try again. */
                prvUnlockQueue(pxQueue);
                (void)xTaskResumeAll();
            }
        } else {
            prvUnlockQueue(pxQueue);
            (void)xTaskResumeAll();
            traceQUEUE_RECEIVE_FAILED(pxQueue);
            return errQUEUE_EMPTY;
        }
    }
}

/*-----------------------------------------------------------*/

signed portBASE_TYPE xQueueReceiveFromISR(xQueueHandle pxQueueParam,
                                          void *const pvBuffer,
                                          signed portBASE_TYPE * pxTaskWoken)
{
    signed portBASE_TYPE xReturn;
    unsigned portBASE_TYPE uxSavedInterruptStatus;
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    uxSavedInterruptStatus = portSET_INTERRUPT_MASK_FROM_ISR();
    {
        /* We cannot block from an ISR, so check there is data available. */
        if (pxQueue->uxMessagesWaiting > (unsigned portBASE_TYPE)0) {
            traceQUEUE_RECEIVE_FROM_ISR(pxQueue);

            prvCopyDataFromQueue(pxQueue, pvBuffer);
            --(pxQueue->uxMessagesWaiting);

            /* If the queue is locked we will not modify the event list.
             * Instead we update the lock count so the task that unlocks the
             * queue will know that an ISR has removed data while the queue was
             * locked. */
            if (pxQueue->xRxLock == queueUNLOCKED) {
                if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToSend))) {
                    if (xTaskRemoveFromEventList
                        (&(pxQueue->xTasksWaitingToSend)) != pdFALSE) {
                        /* The task waiting has a higher priority than us so
                         * force a context switch. */
                        *pxTaskWoken = pdTRUE;
                    }
                }
            } else {
                /* Increment the lock count so the task that unlocks the queue
                 * knows that data was removed while it was locked. */
                ++(pxQueue->xRxLock);
            }

            xReturn = pdPASS;
        } else {
            xReturn = pdFAIL;
            traceQUEUE_RECEIVE_FROM_ISR_FAILED(pxQueue);
        }
    }
    portCLEAR_INTERRUPT_MASK_FROM_ISR(uxSavedInterruptStatus);

    return xReturn;
}

/*-----------------------------------------------------------*/

unsigned portBASE_TYPE uxQueueMessagesWaiting(const xQueueHandle pxQueueParam)
{
    unsigned portBASE_TYPE uxReturn;
    const xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    taskENTER_CRITICAL();
    uxReturn = pxQueue->uxMessagesWaiting;
    taskEXIT_CRITICAL();

    return uxReturn;
}

/*-----------------------------------------------------------*/

unsigned portBASE_TYPE uxQueueMessagesWaitingFromISR(const xQueueHandle
                                                     pxQueueParam)
{
    unsigned portBASE_TYPE uxReturn;
    const xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    uxReturn = pxQueue->uxMessagesWaiting;

    return uxReturn;
}

/*-----------------------------------------------------------*/

void vQueueDelete(xQueueHandle pxQueueParam)
{
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;
    traceQUEUE_DELETE(pxQueue);
    vQueueUnregisterQueue(pxQueue);
    vPortFree(pxQueue->pcHead);
    vPortFree(pxQueue);
}

/*-----------------------------------------------------------*/

static void prvCopyDataToQueue(xQUEUE * pxQueue, const void *pvItemToQueue,
                               portBASE_TYPE xPosition)
{
    if (pxQueue->uxItemSize == (unsigned portBASE_TYPE)0) {
#if ( configUSE_MUTEXES == 1 )
        {
            if (pxQueue->uxQueueType == queueQUEUE_IS_MUTEX) {
                /* The mutex is no longer being held. */
                vTaskPriorityDisinherit((void *)pxQueue->pxMutexHolder);
                pxQueue->pxMutexHolder = NULL;
            }
        }
#endif
    } else if (xPosition == queueSEND_TO_BACK) {
        memcpy((void *)pxQueue->pcWriteTo, pvItemToQueue,
               (unsigned)pxQueue->uxItemSize);
        pxQueue->pcWriteTo += pxQueue->uxItemSize;
        if (pxQueue->pcWriteTo >= pxQueue->pcTail) {
            pxQueue->pcWriteTo = pxQueue->pcHead;
        }
    } else {
        memcpy((void *)pxQueue->pcReadFrom, pvItemToQueue,
               (unsigned)pxQueue->uxItemSize);
        pxQueue->pcReadFrom -= pxQueue->uxItemSize;
        if (pxQueue->pcReadFrom < pxQueue->pcHead) {
            pxQueue->pcReadFrom = (pxQueue->pcTail - pxQueue->uxItemSize);
        }
    }

    ++(pxQueue->uxMessagesWaiting);
}

/*-----------------------------------------------------------*/

static void prvCopyDataFromQueue(xQUEUE * const pxQueue, const void *pvBuffer)
{
    if (pxQueue->uxQueueType != queueQUEUE_IS_MUTEX) {
        pxQueue->pcReadFrom += pxQueue->uxItemSize;
        if (pxQueue->pcReadFrom >= pxQueue->pcTail) {
            pxQueue->pcReadFrom = pxQueue->pcHead;
        }
        memcpy((void *)pvBuffer, (void *)pxQueue->pcReadFrom,
               (unsigned)pxQueue->uxItemSize);
    }
}

/*-----------------------------------------------------------*/

static void prvUnlockQueue(xQueueHandle pxQueueParam)
{
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;
    /* THIS FUNCTION MUST BE CALLED WITH THE SCHEDULER SUSPENDED. */

    /* The lock counts contains the number of extra data items placed or
     * removed from the queue while the queue was locked.  When a queue is
     * locked items can be added or removed, but the event lists cannot be
     * updated. */
    taskENTER_CRITICAL();
    {
        /* See if data was added to the queue while it was locked. */
        while (pxQueue->xTxLock > queueLOCKED_UNMODIFIED) {
            /* Data was posted while the queue was locked.  Are any tasks
             * blocked waiting for data to become available? */
            if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToReceive))) {
                /* Tasks that are removed from the event list will get added to
                 * the pending ready list as the scheduler is still suspended. */
                if (xTaskRemoveFromEventList(&(pxQueue->xTasksWaitingToReceive))
                    != pdFALSE) {
                    /* The task waiting has a higher priority so record that a
                     * context switch is required. */
                    vTaskMissedYield();
                }

                --(pxQueue->xTxLock);
            } else {
                break;
            }
        }

        pxQueue->xTxLock = queueUNLOCKED;
    }
    taskEXIT_CRITICAL();

    /* Do the same for the Rx lock. */
    taskENTER_CRITICAL();
    {
        while (pxQueue->xRxLock > queueLOCKED_UNMODIFIED) {
            if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToSend))) {
                if (xTaskRemoveFromEventList(&(pxQueue->xTasksWaitingToSend)) !=
                    pdFALSE) {
                    vTaskMissedYield();
                }

                --(pxQueue->xRxLock);
            } else {
                break;
            }
        }

        pxQueue->xRxLock = queueUNLOCKED;
    }
    taskEXIT_CRITICAL();
}

/*-----------------------------------------------------------*/

static signed portBASE_TYPE prvIsQueueEmpty(const xQueueHandle pxQueueParam)
{
    const xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;
    signed portBASE_TYPE xReturn;

    taskENTER_CRITICAL();
    xReturn = (pxQueue->uxMessagesWaiting == (unsigned portBASE_TYPE)0);
    taskEXIT_CRITICAL();

    return xReturn;
}

/*-----------------------------------------------------------*/

signed portBASE_TYPE xQueueIsQueueEmptyFromISR(const xQueueHandle pxQueueParam)
{
    signed portBASE_TYPE xReturn;
    const xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    xReturn = (pxQueue->uxMessagesWaiting == (unsigned portBASE_TYPE)0);

    return xReturn;
}

/*-----------------------------------------------------------*/

static signed portBASE_TYPE prvIsQueueFull(const xQueueHandle pxQueueParam)
{
    signed portBASE_TYPE xReturn;
    const xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    taskENTER_CRITICAL();
    xReturn = (pxQueue->uxMessagesWaiting == pxQueue->uxLength);
    taskEXIT_CRITICAL();

    return xReturn;
}

/*-----------------------------------------------------------*/

signed portBASE_TYPE xQueueIsQueueFullFromISR(const xQueueHandle pxQueueParam)
{
    signed portBASE_TYPE xReturn;
    const xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    xReturn = (pxQueue->uxMessagesWaiting == pxQueue->uxLength);

    return xReturn;
}

/*-----------------------------------------------------------*/

#if configUSE_CO_ROUTINES == 1
signed portBASE_TYPE xQueueCRSend(xQueueHandle pxQueueParam,
                                  const void *pvItemToQueue,
                                  portTickType xTicksToWait)
{
    signed portBASE_TYPE xReturn;
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    /* If the queue is already full we may have to block.  A critical section
     * is required to prevent an interrupt removing something from the queue
     * between the check to see if the queue is full and blocking on the queue. */
    portDISABLE_INTERRUPTS();
    {
        if (prvIsQueueFull(pxQueue)) {
            /* The queue is full - do we want to block or just leave without
             * posting? */
            if (xTicksToWait > (portTickType) 0) {
                /* As this is called from a coroutine we cannot block directly,
                 * but return indicating that we need to block. */
                vCoRoutineAddToDelayedList(xTicksToWait,
                                           &(pxQueue->xTasksWaitingToSend));
                portENABLE_INTERRUPTS();
                return errQUEUE_BLOCKED;
            } else {
                portENABLE_INTERRUPTS();
                return errQUEUE_FULL;
            }
        }
    }
    portENABLE_INTERRUPTS();

    portNOP();

    portDISABLE_INTERRUPTS();
    {
        if (pxQueue->uxMessagesWaiting < pxQueue->uxLength) {
            /* There is room in the queue, copy the data into the queue. */
            prvCopyDataToQueue(pxQueue, pvItemToQueue, queueSEND_TO_BACK);
            xReturn = pdPASS;

            /* Were any co-routines waiting for data to become available? */
            if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToReceive))) {
                /* In this instance the co-routine could be placed directly
                 * into the ready list as we are within a critical section.
                 * Instead the same pending ready list mechanism is used as if
                 * the event were caused from within an interrupt. */
                if (xCoRoutineRemoveFromEventList
                    (&(pxQueue->xTasksWaitingToReceive)) != pdFALSE) {
                    /* The co-routine waiting has a higher priority so record
                     * that a yield might be appropriate. */
                    xReturn = errQUEUE_YIELD;
                }
            }
        } else {
            xReturn = errQUEUE_FULL;
        }
    }
    portENABLE_INTERRUPTS();

    return xReturn;
}
#endif

/*-----------------------------------------------------------*/

#if configUSE_CO_ROUTINES == 1
signed portBASE_TYPE xQueueCRReceive(xQueueHandle pxQueueParam, void *pvBuffer,
                                     portTickType xTicksToWait)
{
    signed portBASE_TYPE xReturn;
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    /* If the queue is already empty we may have to block.  A critical section
     * is required to prevent an interrupt adding something to the queue
     * between the check to see if the queue is empty and blocking on the
     * queue. */
    portDISABLE_INTERRUPTS();
    {
        if (pxQueue->uxMessagesWaiting == (unsigned portBASE_TYPE)0) {
            /* There are no messages in the queue, do we want to block or just
             * leave with nothing? */
            if (xTicksToWait > (portTickType) 0) {
                /* As this is a co-routine we cannot block directly, but return
                 * indicating that we need to block. */
                vCoRoutineAddToDelayedList(xTicksToWait,
                                           &(pxQueue->xTasksWaitingToReceive));
                portENABLE_INTERRUPTS();
                return errQUEUE_BLOCKED;
            } else {
                portENABLE_INTERRUPTS();
                return errQUEUE_FULL;
            }
        }
    }
    portENABLE_INTERRUPTS();

    portNOP();

    portDISABLE_INTERRUPTS();
    {
        if (pxQueue->uxMessagesWaiting > (unsigned portBASE_TYPE)0) {
            /* Data is available from the queue. */
            pxQueue->pcReadFrom += pxQueue->uxItemSize;
            if (pxQueue->pcReadFrom >= pxQueue->pcTail) {
                pxQueue->pcReadFrom = pxQueue->pcHead;
            }
            --(pxQueue->uxMessagesWaiting);
            memcpy((void *)pvBuffer, (void *)pxQueue->pcReadFrom,
                   (unsigned)pxQueue->uxItemSize);

            xReturn = pdPASS;

            /* Were any co-routines waiting for space to become available? */
            if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToSend))) {
                /* In this instance the co-routine could be placed directly
                 * into the ready list as we are within a critical section.
                 * Instead the same pending ready list mechanism is used as if
                 * the event were caused from within an interrupt. */
                if (xCoRoutineRemoveFromEventList
                    (&(pxQueue->xTasksWaitingToSend)) != pdFALSE) {
                    xReturn = errQUEUE_YIELD;
                }
            }
        } else {
            xReturn = pdFAIL;
        }
    }
    portENABLE_INTERRUPTS();

    return xReturn;
}
#endif

/*-----------------------------------------------------------*/



#if configUSE_CO_ROUTINES == 1
signed portBASE_TYPE xQueueCRSendFromISR(xQueueHandle pxQueueParam,
                                         const void *pvItemToQueue,
                                         signed portBASE_TYPE
                                         xCoRoutinePreviouslyWoken)
{
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    /* Cannot block within an ISR so if there is no space on the queue then
     * exit without doing anything. */
    if (pxQueue->uxMessagesWaiting < pxQueue->uxLength) {
        prvCopyDataToQueue(pxQueue, pvItemToQueue, queueSEND_TO_BACK);

        /* We only want to wake one co-routine per ISR, so check that a
         * co-routine has not already been woken. */
        if (!xCoRoutinePreviouslyWoken) {
            if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToReceive))) {
                if (xCoRoutineRemoveFromEventList
                    (&(pxQueue->xTasksWaitingToReceive)) != pdFALSE) {
                    return pdTRUE;
                }
            }
        }
    }

    return xCoRoutinePreviouslyWoken;
}
#endif

/*-----------------------------------------------------------*/

#if configUSE_CO_ROUTINES == 1
signed portBASE_TYPE xQueueCRReceiveFromISR(xQueueHandle pxQueueParam,
                                            void *pvBuffer,
                                            signed portBASE_TYPE *
                                            pxCoRoutineWoken)
{
    signed portBASE_TYPE xReturn;
    xQueueHandlePrv pxQueue = (xQueueHandlePrv) pxQueueParam;

    /* We cannot block from an ISR, so check there is data available. If not
     * then just leave without doing anything. */
    if (pxQueue->uxMessagesWaiting > (unsigned portBASE_TYPE)0) {
        /* Copy the data from the queue. */
        pxQueue->pcReadFrom += pxQueue->uxItemSize;
        if (pxQueue->pcReadFrom >= pxQueue->pcTail) {
            pxQueue->pcReadFrom = pxQueue->pcHead;
        }
        --(pxQueue->uxMessagesWaiting);
        memcpy((void *)pvBuffer, (void *)pxQueue->pcReadFrom,
               (unsigned)pxQueue->uxItemSize);

        if (!(*pxCoRoutineWoken)) {
            if (!listLIST_IS_EMPTY(&(pxQueue->xTasksWaitingToSend))) {
                if (xCoRoutineRemoveFromEventList
                    (&(pxQueue->xTasksWaitingToSend)) != pdFALSE) {
                    *pxCoRoutineWoken = pdTRUE;
                }
            }
        }

        xReturn = pdPASS;
    } else {
        xReturn = pdFAIL;
    }

    return xReturn;
}
#endif

/*-----------------------------------------------------------*/

#if configQUEUE_REGISTRY_SIZE > 0

void vQueueAddToRegistry(xQueueHandle xQueueParam, signed char *pcQueueName)
{
    unsigned portBASE_TYPE ux;
    xQueueHandlePrv xQueue = (xQueueHandlePrv) xQueueParam;

    /* See if there is an empty space in the registry.  A NULL name denotes a
     * free slot. */
    for (ux = 0; ux < configQUEUE_REGISTRY_SIZE; ux++) {
        if (xQueueRegistry[ux].pcQueueName == NULL) {
            /* Store the information on this queue. */
            xQueueRegistry[ux].pcQueueName = pcQueueName;
            xQueueRegistry[ux].xHandle = xQueue;
            break;
        }
    }
}

#endif

    /*-----------------------------------------------------------*/

#if configQUEUE_REGISTRY_SIZE > 0

static void vQueueUnregisterQueue(xQueueHandle xQueueParam)
{
    unsigned portBASE_TYPE ux;
    xQueueHandlePrv xQueue = (xQueueHandlePrv) xQueueParam;

    /* See if the handle of the queue being unregistered in actually in the
     * registry. */
    for (ux = 0; ux < configQUEUE_REGISTRY_SIZE; ux++) {
        if (xQueueRegistry[ux].xHandle == xQueue) {
            /* Set the name to NULL to show that this slot if free again. */
            xQueueRegistry[ux].pcQueueName = NULL;
            break;
        }
    }

}

#endif
